#include <vector>

#include <boost/functional/hash.hpp>
#include <boost/test/execution_monitor.hpp>

#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoFaceSet.h>
#include <Inventor/nodes/SoMaterial.h>
#include <Inventor/nodes/SoTransform.h>
#include <Inventor/nodes/SoSphere.h>
#include <Inventor/nodes/SoLineSet.h>
//#include <Inventor/annex/FXViz/nodes/SoShadowCulling.h>

#include "marching_cubes.hpp"
#include "marching_cubes_data.hpp"
#include "point.hpp"

#include <iostream>

//using std::assert
using std::vector;
using std::abs;
using std::sqrt;

using boost::hash;

using std::cerr;
using std::cout;
using std::endl;


static unsigned long nx,ny,nz;



size_t hash_node(long ix, long iy, long iz);


noderef::noderef(long x, long y, long z, int i)
{
  Set(x,y,z,i);
}



void noderef::Set(long x, long y, long z, int i)
{
    switch (i)
    {
    case 0:
        ix=0;
        iy=0;
        iz=0;
        break;
    case 1:
        ix=1;
        iy=0;
        iz=0;
        break;
    case 2:
        ix=1;
        iy=1;
        iz=0;
        break;
    case 3:
        ix=0;
        iy=1;
        iz=0;
        break;
    case 4:
        ix=0;
        iy=0;
        iz=1;
        break;
    case 5:
        ix=1;
        iy=0;
        iz=1;
        break;
    case 6:
        ix=1;
        iy=1;
        iz=1;
        break;
    case 7:
        ix=0;
        iy=1;
        iz=1;
        break;
    default:
        assert(false);
    }
    ix+=x;
    iy+=y;
    iz+=z;
}

noderef::operator size_t() const
{
    return hash_node(ix,iy,iz);
}






// template<typename T>inline void swap(T &a, T &b)
// {
//     T c;
//     c=a;
//     a=b;
//     b=c;
// }
template<typename T>inline void swap3(T &x1, T &y1, T &z1,
                                      T &x2, T &y2, T &z2)
{
    swap(x1,x2);
    swap(y1,y2);
    swap(z1,z2);
}


size_t hash_edge(long ix1,long iy1, long iz1,
                 long ix2,long iy2, long iz2)
{

    // assert(abs(ix1-ix2)==1 && abs(iy1-iy2)==1 && abs(iz1-iz2)==1);


    /*f(abs(ix1-ix2)!=1)
      cerr<<"ix1: "<<ix1<<"ix2: "<<ix2<<endl;
    f(abs(iy1-iy2)!=1)
      cerr<<"iy1: "<<iy1<<"iy2: "<<iy2<<endl; 
    f(abs(iz1-iz2)!=1)
      cerr<<"iz1: "<<iz1<<"iz2: "<<iz2<<endl; */



    if ((ix1>ix2) || (iy1>iy2) || (iz1>iz2))
        swap3(ix1,iy1,iz1,ix2,iy2,iz2);
    /*
    vector<long> v;
    v.push_back(ix1);
    v.push_back(iy1);
    v.push_back(iz1);
    v.push_back(ix2);
    v.push_back(iy2);
    v.push_back(iz2);

    hash<vector<long> > long_hash;

    return long_hash(v); */


    //!!TODO better hash algo
    return hash_node(ix1,iy1,iz1)*nz*nx*ny+hash_node(ix2,iy2,iz2);

}

size_t hash_node(long ix, long iy, long iz)
{
    /*vector<long> v;
    v.push_back(ix);
    v.push_back(iy);
    v.push_back(iz);

    hash<vector<long> > long_hash;

    return long_hash(v);
    */


    return iz*nx*ny+iy*ny+ix;

}



Facetter::Facetter(dexelfield *m)
{
    model=m;


    dx=0.25;
    dy=0.25;
    dz=0.25;

    xmin=m->xmin-1.5*dx;
    xmax=m->xmax+1.5*dx;

    ymin=m->ymin-1.5*dy;
    ymax=m->ymax+1.5*dy;

    zmin=m->zmin-1.5*dz;
    zmax=m->zmax+1.5*dz;

    cerr<<"model bounds\n";
    cerr<<"x: "<<xmin<<" "<<xmax<<endl;
    cerr<<"y: "<<ymin<<" "<<ymax<<endl;
    cerr<<"z: "<<zmin<<" "<<zmax<<endl;


    nx=(unsigned long)floor(abs(xmax-xmin)/dx)+10,
       ny=(unsigned long)floor(abs(ymax-ymin)/dy)+10,
          nz=(unsigned long)floor(abs(zmax-zmin)/dz)+10;

    volume_quant=dx*dy*dz/1000;


    result=new SoSeparator;
    result->ref();

}
void Facetter::setseed(double _dx, double  _dy, double _dz)
{
    dx=_dx;
    dy=_dy;
    dz=_dz;

    xmin=model->xmin-1.5*dx;
    xmax=model->xmax+1.5*dx;

    ymin=model->ymin-1.5*dy;
    ymax=model->ymax+1.5*dy;

    zmin=model->zmin-1.5*dz;
    zmax=model->zmax+1.5*dz;

    nx=(unsigned long)floor(abs(xmax-xmin)/dx)+10,
       ny=(unsigned long)floor(abs(ymax-ymin)/dy)+10,
          nz=(unsigned long)floor(abs(zmax-zmin)/dz)+10;

}


void Facetter::setbounds(double _xmin, double _xmax,
                         double _ymin, double _ymax,
                         double _zmin, double _zmax)
{
    xmin=_xmin;
    xmax=_xmax;

    ymin=_ymin;
    ymax=_ymax;

    zmin=_zmin;
    zmax=_zmax;

}




body_state Facetter::test_state(double x,double y, double z)
{
    return model->state(x,y,z);

}


point Facetter::intersect(double x1,double y1, double z1,
                          double x2,double y2, double z2)
{

    double x,y,z;
    if ( abs(x1-x2)<dx/100 &&
            abs(y1-y2)<dx/100 )
    {
        x=x1;
        y=y1;
        z=model->intersect_z(x,y,z1,z2);
    }
    if ( abs(x1-x2)<dx/100 &&
            abs(z1-z2)<dx/100 )
    {
        x=x1;
        z=z1;
        y=model->intersect_y(x,y1,z,y2);
    }
    if ( abs(z1-z2)<dx/100 &&
            abs(y1-y2)<dx/100 )
    {
        z=z1;
        y=y1;
        x=model->intersect_x(x1,y,z,x2);
    }



    return point(x,y,z);

}

void Facetter::refacet(double x0 ,double x1,
                       double y0, double y1,
                       double z0, double z1,
                       facet_note *tic)
{
    facet_note dim;
    if (tic==NULL)
        tic=&dim;

    if (x0<xmin)
        x0=xmin;
    if (y0<ymin)
        y0=ymin;
    if (z0<zmin)
        z0=zmin;

    if (x1>xmax)
        x1=xmax;
    if (y1>ymax)
        y1=ymax;
    if (z1>zmax)
        z1=zmax;


    x0=floor((x0-xmin)/dx)*dx+xmin;
    y0=floor((y0-ymin)/dy)*dy+ymin;
    z0=floor((z0-zmin)/dz)*dz+zmin;



    double pp=
        ceil((x1-x0)/dx)*
        ceil((y1-y0)/dy)*
        ceil((z1-z0)/dz);
    cerr<<"points to check: "<<pp<<endl;
    double pc=0;
    int percents=0;

    for (double x=x0; x<=x1; x+=dx)
        for (double y=y0; y<=y1; y+=dy)
            for (double z=z0; z<=z1; z+=dz)
            {
                long ix=long((x-xmin)/dx);
                long iy=long((y-ymin)/dy);
                long iz=long((z-zmin)/dz);




                nodes[hash_node(ix,iy,iz)]=test_state(x,y,z);

                if ((++pc)/pp*100>percents+1)
                    //cerr<<"nodes checked "<<++percents<<"%\r";
                    tic->tic("nodes checked",++percents);

            }
    pc=0;
    cerr<<endl;

    percents=0;
    
    // Result()->enableNotify(false);
    
    for (double x=x0; x<=x1; x+=dx)
        for (double y=y0; y<=y1; y+=dy)
            for (double z=z0; z<=z1; z+=dz)
            {
                long ix=long((x-xmin)/dx);
                long iy=long((y-ymin)/dy);
                long iz=long((z-zmin)/dz);

                //size_t cube_hash=hash_node(ix,iy,iz);

                // if (cubes[cube_hash]!=NULL)
//                 {
//                     cubes[cube_hash]->removeAllChildren();
//                 }

                facet_cube(ix,iy,iz);

                if ((++pc)/pp*100>percents+1)
                    // cerr<<"cubes faceted "<<++percents<<"%\r";
                    tic->tic("cubes faceted ",++percents);

            }
    //  Result()->enableNotify(true);
   
    cerr<<endl;


}

void Facetter::facet_traverse(facet_note *tic)
{
  facet_note dim;
  if (tic==NULL)
    tic=&dim;
  
    double pp=
      ceil((xmax-xmin)/dx)*
      ceil((ymax-ymin)/dy)*
      ceil((zmax-zmin)/dz);
    cerr<<"points to check: "<<pp<<endl;
    double pc=0;
    int percents=0;

    for (double x=xmin; x<=xmax; x+=dx)
      for (double y=ymin; y<=ymax; y+=dy)
	for (double z=zmin; z<=zmax; z+=dz)
	  {
                long ix=long((x-xmin)/dx);
                long iy=long((y-ymin)/dy);
                long iz=long((z-zmin)/dz);

                nodes[hash_node(ix,iy,iz)]=test_state(x,y,z);

                if ((++pc)/pp*100>percents+1)
                    //cerr<<"nodes checked "<<++percents<<"%\r";
                    tic->tic("nodes checked",++percents);

            }
    pc=0;
    cerr<<endl;

    percents=0;

    SoCoordinate3 *coords=new SoCoordinate3;
    SoFaceSet *faces=new SoFaceSet;
    

      long vert=0,tri=0;
      
    for (double x=xmin; x<=xmax; x+=dx)
        for (double y=ymin; y<=ymax; y+=dy)
            for (double z=zmin; z<=zmax; z+=dz)
            {
                long ix=long((x-xmin)/dx);
                long iy=long((y-ymin)/dy);
                long iz=long((z-zmin)/dz);

		facet_cube_traverse(ix,iy,iz,
				    coords,faces,
				    vert,tri);

                if ((++pc)/pp*100>percents+1)
		  tic->tic("cubes faceted ",++percents);

            }
    
    Result()->removeAllChildren();
    Result()->addChild(coords);
    Result()->addChild(faces);
   
    cerr<<endl;


}


void Facetter::facet(facet_note *tic)
{
    refacet(xmin,xmax,
            ymin,ymax,
            zmin,zmax,tic);


}


size_t hash_edge(noderef n1, noderef n2)
{
    return hash_edge(n1.ix, n1.iy, n1.iz,
                     n2.ix, n2.iy, n2.iz);
}

size_t hash_tetro(long ix,long iy,long iz,
                  int v0, int v1,int v2,int v3)
{
    vector<long> v;
    v.push_back(ix);
    v.push_back(iy);
    v.push_back(iz);
    v.push_back(v0);
    v.push_back(v1);
    v.push_back(v2);
    v.push_back(v3);


    hash<vector<long> > long_hash;

    return long_hash(v);
}



point Facetter::intersect(noderef n1, noderef n2)
{
    point &p=edges[hash_edge(n1,n2)];

    if (p.known())
        return p;

    double x1=xmin+n1.ix*dx;
    double y1=ymin+n1.iy*dy;
    double z1=zmin+n1.iz*dz;

    double x2=xmin+n2.ix*dx;
    double y2=ymin+n2.iy*dy;
    double z2=zmin+n2.iz*dz;

//  SoCoordinate3 *coords=new SoCoordinate3;
//   coords->point.set1Value(0,x1,y1,z1);
//   coords->point.set1Value(1,x2,y2,z2);
//   SoLineSet *lines=new SoLineSet;
//   lines->numVertices.set1Value(0,2);

//   SoSeparator *s=new SoSeparator;
//   s->ref();

//   s->addChild(coords);
//   s->addChild(lines);

//   result->addChild(s);


    p=intersect(x1,y1,z1,
                x2,y2,z2);
    return p;

}



void Facetter::facet_cube(long ix,long iy,long iz)
{
   
    noderef cube[8];

    for (int i=0; i<8; i++)
        cube[i].Set(ix,iy,iz,i);
    /*
        Determine the index into the edge table which
        tells us which vertices are inside of the surface
     */
    int cubeindex=0,bit=1;
    for (int i=0; i<8; i++)
    {
        if (nodes[cube[i]]==SOLID)
            cubeindex |= bit;
        bit*=2;
    }


    /* Cube is entirely in/out of the surface */
    if (edgeTable[cubeindex] == 0)
      {
	size_t cube_hash=hash_node(ix,iy,iz);
	if (cubes[cube_hash]!=NULL)
	  cubes[cube_hash]->removeAllChildren();
	return;
      }
    

    point vertlist[12];


    /* Find the vertices where the surface intersects the cube */
    if (edgeTable[cubeindex] & 1)
        vertlist[0] = intersect(cube[0],cube[1]);
    if (edgeTable[cubeindex] & 2)
        vertlist[1] = intersect(cube[1],cube[2]);
    if (edgeTable[cubeindex] & 4)
        vertlist[2] = intersect(cube[2],cube[3]);
    if (edgeTable[cubeindex] & 8)
        vertlist[3] = intersect(cube[3],cube[0]);
    if (edgeTable[cubeindex] & 16)
        vertlist[4] = intersect(cube[4],cube[5]);
    if (edgeTable[cubeindex] & 32)
        vertlist[5] = intersect(cube[5],cube[6]);
    if (edgeTable[cubeindex] & 64)
        vertlist[6] = intersect(cube[6],cube[7]);
    if (edgeTable[cubeindex] & 128)
        vertlist[7] = intersect(cube[7],cube[4]);
    if (edgeTable[cubeindex] & 256)
        vertlist[8] = intersect(cube[0],cube[4]);
    if (edgeTable[cubeindex] & 512)
        vertlist[9] = intersect(cube[1],cube[5]);
    if (edgeTable[cubeindex] & 1024)
        vertlist[10] = intersect(cube[2],cube[6]);
    if (edgeTable[cubeindex] & 2048)
        vertlist[11] = intersect(cube[3],cube[7]);

    /* Create the triangle */

    size_t cube_hash=hash_node(ix,iy,iz);

    SoSeparator *grid;

    if (cubes[cube_hash]==NULL)
    {
        grid= new SoSeparator;
        grid->ref();
        cubes[cube_hash]=grid;

        result->addChild(grid);
    }
    else
        grid=cubes[cube_hash];

    

    SoCoordinate3 *coords=new SoCoordinate3;
    SoFaceSet *faces=new SoFaceSet;

    


    int tri = 0,vert=0;
    for (int i=0;triTable[cubeindex][i]!=-1;i+=3)
    {
        coords->point.set1Value(vert++,vertlist[triTable[cubeindex][i+2]]);
        coords->point.set1Value(vert++,vertlist[triTable[cubeindex][i+1]]);
        coords->point.set1Value(vert++,vertlist[triTable[cubeindex][i  ]]);
        faces->numVertices.set1Value(tri++,3);

    }
    
   grid->removeAllChildren();
   grid->addChild(coords);
   grid->addChild(faces);
   
}

void Facetter::facet_cube_traverse(long ix,long iy,long iz,
				   SoCoordinate3 *coords,
				   SoFaceSet *faces,
				   long &vert, long &tri)
{
   
    noderef cube[8];

    for (int i=0; i<8; i++)
        cube[i].Set(ix,iy,iz,i);
    /*
        Determine the index into the edge table which
        tells us which vertices are inside of the surface
     */
    int cubeindex=0,bit=1;
    for (int i=0; i<8; i++)
    {
        if (nodes[cube[i]]==SOLID)
            cubeindex |= bit;
        bit*=2;
    }


    /* Cube is entirely in/out of the surface */
    if(edgeTable[cubeindex] == 0)
      return;
    
    

    point vertlist[12];


    /* Find the vertices where the surface intersects the cube */
    if (edgeTable[cubeindex] & 1)
        vertlist[0] = intersect(cube[0],cube[1]);
    if (edgeTable[cubeindex] & 2)
        vertlist[1] = intersect(cube[1],cube[2]);
    if (edgeTable[cubeindex] & 4)
        vertlist[2] = intersect(cube[2],cube[3]);
    if (edgeTable[cubeindex] & 8)
        vertlist[3] = intersect(cube[3],cube[0]);
    if (edgeTable[cubeindex] & 16)
        vertlist[4] = intersect(cube[4],cube[5]);
    if (edgeTable[cubeindex] & 32)
        vertlist[5] = intersect(cube[5],cube[6]);
    if (edgeTable[cubeindex] & 64)
        vertlist[6] = intersect(cube[6],cube[7]);
    if (edgeTable[cubeindex] & 128)
        vertlist[7] = intersect(cube[7],cube[4]);
    if (edgeTable[cubeindex] & 256)
        vertlist[8] = intersect(cube[0],cube[4]);
    if (edgeTable[cubeindex] & 512)
        vertlist[9] = intersect(cube[1],cube[5]);
    if (edgeTable[cubeindex] & 1024)
        vertlist[10] = intersect(cube[2],cube[6]);
    if (edgeTable[cubeindex] & 2048)
        vertlist[11] = intersect(cube[3],cube[7]);

    /* Create the triangle */

    size_t cube_hash=hash_node(ix,iy,iz);

  
    


    for (int i=0;triTable[cubeindex][i]!=-1;i+=3)
    {
        coords->point.set1Value(vert++,vertlist[triTable[cubeindex][i+2]]);
        coords->point.set1Value(vert++,vertlist[triTable[cubeindex][i+1]]);
        coords->point.set1Value(vert++,vertlist[triTable[cubeindex][i  ]]);
        faces->numVertices.set1Value(tri++,3);

    }
   
}


